#include <iostream>
#include <random>
#include <sstream>
#include <stdexcept>
#include <string>
#include <thread>
#include <tuple>
#include <chrono>
#include <vector>
#include <fstream>
#include <sstream>
#include <cstring>
#include <functional>

#include "carla/client/Client.h"
#include "carla/trafficmanager/TrafficManager.h"
#include "carla/client/BlueprintLibrary.h"
#include "carla/client/Map.h"
#include "carla/client/World.h"
#include "carla/image/ImageIO.h"
#include "carla/image/ImageView.h"
#include "carla/sensor/data/Image.h"
#include "carla/client/Sensor.h"
#include "rclcpp/rclcpp.hpp"
#include "carla_ros2_interface/msg/rpc_vehicle_control.hpp"

using namespace std::chrono_literals;
using namespace std::string_literals;

/**
 * @brief 打印多个 std::string 信息
 * 
 * @param string_vector 
 */
void LogMany(const std::vector<std::string>& string_vector);
void LogMany(const std::vector<carla::geom::Transform>& transfor_vector);
void OnRecvSensorImageData(const carla::sensor::data::Image& image);
void FileWrite(std::fstream& f_for_write, const std::string& file_name, const std::string& s);
void Init();
void OnRecvVehicleControlRequest(
    const carla_ros2_interface::msg::RpcVehicleControl::SharedPtr rpc_vehicle_control,
    carla::client::Vehicle& vehicle);

/*
全局变量区
*/
std::fstream FILE_LOG;
std::string FILE_NAME;

int main(int argc, char const *argv[])
{
    using namespace std::placeholders;

    Init();


    /* code */
    // 建立与客户端的连接
    auto carla_client_ = carla::client::Client("127.0.0.1", 2000);
    carla_client_.SetTimeout(10s);
    auto carla_world_ = carla_client_.GetWorld();
    RCLCPP_INFO(rclcpp::get_logger("carla_test"), "创建 carla client");
    // carla_world_.

    // 获取 world 当前的设置并设置为同步模式
    auto carla_original_settings_ = carla_world_.GetSettings();
    auto carla_new_settings_ = carla_world_.GetSettings();

    try
    {
        carla_new_settings_.synchronous_mode = false;
        carla_world_.ApplySettings(carla_new_settings_, 0s);
        auto traffic_manager_ = carla_client_.GetInstanceTM();
        traffic_manager_.SetSynchronousMode(true);

        // 查找车辆蓝图 >>>>
        LogMany(carla_client_.GetAvailableMaps());    // 打印当前所有的地图名
        auto blueprint_lib_ = carla_world_.GetBlueprintLibrary();
        auto bp_vehicle_model3_ =  blueprint_lib_->Filter("vehicle.tesla.model3");
        assert (bp_vehicle_model3_), "找不到蓝图 vehicle.tesla.model3";

        // 查找出生点并产生车 >>>>>>>
        auto carla_map_ = carla_world_.GetMap();
        auto spawn_points_ = carla_map_->GetRecommendedSpawnPoints();
        auto spawn_point_ = spawn_points_.at(0);
        RCLCPP_INFO(rclcpp::get_logger("carla_test"), "出生点");
        LogMany(spawn_points_);
        auto act_ = carla_world_.SpawnActor(bp_vehicle_model3_->at(0), spawn_point_);
        auto act_vehicle_model3_ = boost::static_pointer_cast<carla::client::Vehicle>(act_);

        // 移动仿真器的视角到车的旁边
        auto spectator_ = carla_world_.GetSpectator();
        auto transform_view_ = carla::geom::Transform(spawn_point_);
        transform_view_.location +=  -5.0f * transform_view_.GetForwardVector();
        transform_view_.location.z += 2.0f;
        transform_view_.rotation.pitch = -15.0f;
        spectator_->SetTransform(transform_view_);

        // 添加一个图像传感器到车辆上
        auto cam_rgb_bp_ = blueprint_lib_->Find("sensor.camera.rgb");
        assert (cam_rgb_bp_), "找不到蓝图 sensor.camera.rgb";
        auto cam_transform_ = carla::geom::Transform(
            carla::geom::Location(3, 0, 1.2),
            carla::geom::Rotation(-10, 0, 0)
        );
        auto actor_ = carla_world_.SpawnActor(*cam_rgb_bp_, cam_transform_, act_vehicle_model3_.get());    // 将摄像头放在车上
        auto cam_actor_ = boost::static_pointer_cast<carla::client::Sensor>(actor_);
        // cam_actor_->Listen(std::make_shared<std::function>(std::bind(OnRecvSensorImageData, _1)));
        cam_actor_->Listen([](auto data) {
            auto image_data_ = boost::static_pointer_cast<carla::sensor::data::Image>(data);
            assert (image_data_), "收到错误的图像数据";
            OnRecvSensorImageData(*image_data_);
        });

        // 设置车辆控制器回调
        std::shared_ptr<rclcpp::Node> ros_node_carla_test_ = rclcpp::Node::make_shared("ros_node_carla_test");
        ros_node_carla_test_->create_subscription<carla_ros2_interface::msg::RpcVehicleControl>(
            "rpc_vehicle_control",
            10,
            [&](const carla_ros2_interface::msg::RpcVehicleControl::SharedPtr msg) {
                OnRecvVehicleControlRequest(msg, *act_vehicle_model3_);
            }
        );

        rclcpp::init(argc, argv);
        rclcpp::spin(ros_node_carla_test_);
        rclcpp::shutdown();

        // std::this_thread::sleep_for(20s);
        // while (true)
        // {
        //     /* code */
        //     std::this_thread::sleep_for(2s);
        // }
        
        act_vehicle_model3_->Destroy();
    }
    catch(const std::exception& e)
    {
        // std::cerr << e.what() << '\n';
        carla_world_.ApplySettings(carla_original_settings_, 0s);
        // TODO 后续可能释放其他资源
        RCLCPP_ERROR(rclcpp::get_logger("carla_test"), e.what());
        return 1;
    }
    return 0;
}


void LogMany(const std::vector<std::string>& string_vector)
{
    for (auto i_: string_vector) {
        RCLCPP_INFO(rclcpp::get_logger("carla_test"), i_);
    }
}

void LogMany(const std::vector<carla::geom::Transform>& transfor_vector)
{
    for (auto i_ : transfor_vector) {
        RCLCPP_INFO(rclcpp::get_logger("carla_test"), 
            "Transform(x:%f,y:%f,z:%f,yaw:%f,roll:%f,pitch:%f),", i_.location.x, i_.location.y, i_.location.z, i_.rotation.yaw, i_.rotation.roll, i_.rotation.pitch);
    }
}

void OnRecvSensorImageData(const carla::sensor::data::Image& image)
{
    auto img_time_ = image.GetTimestamp();
    auto pc_time_ = std::chrono::system_clock::now().time_since_epoch().count();
    // RCLCPP_INFO(rclcpp::get_logger("OnRecvSensorImageData"), 
    //     "image_timestamp: %f, pc_timestamp: %ld", img_time_, pc_time_);
    
    std::ostringstream ss_;
    ss_ << img_time_ << ", " << pc_time_ << '\n';
    // FileWrite(FILE_LOG, FILE_NAME, ss_.str());
}

void FileWrite(std::fstream& f_for_write, const std::string& file_name, const std::string& s)
{
    try
    {
        /* code */
        if (not f_for_write.is_open()) {
            f_for_write.open(file_name, std::ios::out | std::ios::app);
        }
        f_for_write << s;
    }
    catch(const std::exception& e)
    {
        // std::cerr << e.what() << '\n';
        RCLCPP_ERROR(rclcpp::get_logger("WriteCsv"), e.what());
    }
    
    // if (f_for_write)
}

void Init()
{
    std::ostringstream ss_;
    auto time_now_ = std::chrono::system_clock::now().time_since_epoch().count();
    RCLCPP_INFO(rclcpp::get_logger("Init"), "启动时间 ns%ld", time_now_);
    ss_ << "log_file_" << time_now_ << ".csv";
    FILE_NAME = ss_.str(); 
}

void OnRecvVehicleControlRequest(
    const carla_ros2_interface::msg::RpcVehicleControl::SharedPtr rpc_vehicle_control,
    carla::client::Vehicle& vehicle)
{
    // 创建一个车辆控制器 >>>>>>
    carla::client::Vehicle::Control control_(
        rpc_vehicle_control->throttle,
        rpc_vehicle_control->steer,
        rpc_vehicle_control->brake,
        rpc_vehicle_control->hand_brake,
        rpc_vehicle_control->reverse,
        rpc_vehicle_control->manual_gear_shift,
        rpc_vehicle_control->gear
    );
    vehicle.ApplyControl(control_);
}